import sys
import threading
import time
import os
os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0"
import cv2
from PyQt5 import QtCore, QtGui, QtWidgets
from PyQt5.QtWidgets import QFileDialog,QWidget
from PyQt5.QtCore import Qt , QPoint
from PyQt5.QtGui import QCloseEvent
import numpy as np
import json
import math
import serial
import struct
import cauto
from multiprocessing import Pool,Process
import threading
import logging
from logging import handlers
log=logging.getLogger('arucoLog')
log.setLevel(logging.INFO)
fmt=logging.Formatter('%(asctime)s:%(levelname)s:%(funcName)s:%(message)s')
sh=logging.StreamHandler()
sh.setLevel(logging.INFO)
sh.setFormatter(fmt)
log.addHandler(sh)
#fh=logging.FileHandler("auto_fly_attack.log",'w')
fh=handlers.TimedRotatingFileHandler(filename='aruco.txt',when='D')
fh.setLevel(logging.INFO)
fh.setFormatter(fmt)
log.addHandler(fh)

# ser = serial.Serial('COM5', 115200)
class clickableQlabel(QtWidgets.QLabel):
    def __init__(self, parent=None):
        super(clickableQlabel, self).__init__(parent)
        self.point_list=[]
        try:
            with open('pointListLog.txt','r') as f:
                self.point_list=json.load(f)
        except:
            pass
    def mousePressEvent(self, event) :   
    # 鼠标左键按下
        if event.button() == Qt.LeftButton :
            self.lastPoint = event.pos()
            self.endPoint = self.lastPoint
            #print(event.pos())
            self.point_list.append((event.pos().x(), event.pos().y()))
            with open('pointListLog.txt','w') as f:
                json.dump(self.point_list,f)
arucoDict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_50)
arucoParams = cv2.aruco.DetectorParameters_create()
arucoParams.adaptiveThreshConstant=7
arucoParams.adaptiveThreshWinSizeStep=5
# arucoParams.adaptiveThreshWinSizeMax=40
# arucoParams.aprilTagMaxNmaxima=40
# arucoParams.aprilTagCriticalRad=0
# arucoParams.aprilTagMinClusterPixels=0
arucoParams.maxErroneousBitsInBorderRate=0.4
# arucoParams.errorCorrectionRate=0.8
arucoParams.perspectiveRemoveIgnoredMarginPerCell=0.3
arucoParams.polygonalApproxAccuracyRate=0.01
arucoParams.maxMarkerPerimeterRate =4
arucoParams.minCornerDistanceRate=0
arucoParams.minDistanceToBorder=0
arucoParams.minMarkerDistanceRate=0.005 #重要
arucoParams.aprilTagMaxLineFitMse=0.00
angle=0
pos=[0,0,0]
def chooseValue(X,Y,val):# 从小到大
    for x,y in zip(X,Y):
        if val<=x:
            return y
    return Y[-1]
def controlEnd(endPos,stop=True,vpara=([50,250,400,1e18],[30,70,200,300]),errorRange=30):
    # print(pos)
    ser = serial.Serial('COM5', 115200)
    ti=0
    while True:
        if pos==None:
            time.sleep(0.1)
            continue
        if 1000<=pos[0]<1700 and 2400<=pos[1]<3200:
            time.sleep(0.1)
            log.info(f"屏蔽{pos}")
            continue
        try:
            dist=abs(endPos[0]-pos[0])+abs(endPos[1]-pos[1])
            if dist<=errorRange:
                if stop:
                    ser.write(struct.pack('iffifi',2,0,0,0,0,0))
                break
            if time.time()-ti>=0.06:#控制一次的间隔
                ti=time.time()
                newAngle=math.atan2(endPos[1]-pos[1],endPos[0]-pos[0])
                v=chooseValue(vpara[0],vpara[1],dist)
                wheelAngle=newAngle-angle
                while wheelAngle<0:
                    wheelAngle+=math.pi*2 #[0,2pi]
                log.info(f'wheelAngle:{wheelAngle} dist:{dist} v:{v} pos:{pos}')
                ser.write(struct.pack('iffifi',0,-v,wheelAngle,0,0,0))
            time.sleep(0.02)
        except Exception as e:
            print(e)
    ser.close()
def controlRotate(targetAngle):#sleep?
    ser = serial.Serial('COM5', 115200)
    # pack=struct.pack('iffifi',1,0,0,0,200,0)
    # ser.write(pack)
    lastv=0
    while True:
        if angle==None:
            print("None")
            time.sleep(0.1)
            continue
        try:
            if angle<targetAngle:
                if targetAngle-angle<angle+2*math.pi-targetAngle:
                    dist=targetAngle-angle
                    direction=-1
                else:
                    dist=angle+2*math.pi-targetAngle
                    direction=1
            else:
                if angle-targetAngle<targetAngle+2*math.pi-angle:
                    dist=angle-targetAngle
                    direction=1
                else:
                    dist=targetAngle+2*math.pi-angle
                    direction=-1
            dist=dist*180/math.pi
            v=chooseValue([20,40,90,360],[30,60,200,400],dist)*direction
            log.info(f'dist {dist} deg v {v} angle {angle}')
            if v!=lastv:
                ser.write(struct.pack('iffifi',1,0,0,0,v,0))
                lastv=v
            if dist<=5:
                ser.write(struct.pack('iffifi',2,0,0,0,0,0))
                break
            time.sleep(0.003)
        except Exception as e:
            print(e)
    ser.close()
def controlSuckerOn(suckerNumber,back=None):
    ser = serial.Serial('COM5', 115200)
    print(f'sucker{suckerNumber} on')
    steeringEngineID={1:11,2:12,3:13}[suckerNumber]
    cylinderID={1:3,2:5,3:7}[suckerNumber]
    suckerID={1:4,2:6,3:8}[suckerNumber]
    ser.write(struct.pack('iffifi',steeringEngineID,0,0,1,0,0))
    time.sleep(1)
    ser.write(struct.pack('iffifi',cylinderID,0,0,0,0,0))
    time.sleep(1)
    ser.write(struct.pack('iffifi',suckerID,0,0,0,0,0))
    time.sleep(1)
    ser.write(struct.pack('iffifi',cylinderID,0,0,0,0,0))
    time.sleep(1)

    if back!=None:
        ser.write(struct.pack('iffifi',0,-100,back,0,0,0))
        time.sleep(1)
        ser.write(struct.pack('iffifi',2,0,0,0,0,0))
        time.sleep(1)
    ser.write(struct.pack('iffifi',steeringEngineID,0,0,0,0,0))
    time.sleep(1)
def controlSuckerOff(suckerNumber):
    ser = serial.Serial('COM5', 115200)
    print(f'sucker{suckerNumber} off')
    steeringEngineID={1:11,2:12,3:13}[suckerNumber]
    cylinderID={1:3,2:5,3:7}[suckerNumber]
    suckerID={1:4,2:6,3:8}[suckerNumber]

    ser.write(struct.pack('iffifi',cylinderID,0,0,0,0,0))
    time.sleep(1)
    ser.write(struct.pack('iffifi',suckerID,0,0,0,0,0))
    time.sleep(1)
    ser.write(struct.pack('iffifi',cylinderID,0,0,0,0,0))
    time.sleep(1)

suckerOffset={1:[-50,400],2:[400,50]}
def run(para):
    # controlEnd((3300,800),True)
    # controlRotate(-math.pi)
    print("run:",para)
    for i in para:
        if i[0]=='rotate':
            controlRotate(**i[1])
        if i[0]=='move':
            controlEnd(**i[1])
        if i[0]=='controlSuckerOn':
            controlSuckerOn(**i[1])
        if i[0]=='controlSuckerOff':
            controlSuckerOff(**i[1])
def slipshot(obj,rvet,tvet,cameraMatrix,distCoeffs):
    print(obj.shape)
    try:
        re,jac=cv2.projectPoints(obj,rvet,tvet,cameraMatrix,distCoeffs)
        shot=re.reshape(-1,2)
        shot=np.round(shot).astype('int32')
        # print(shot)
    except Exception as e:
        print('error',e)
    return shot
fuelPos3D=np.array([
            [3925,650,110],
            [3925,800,110],
            [3925,950,110],
            [3925,2250,310],
            [3925,2400,310],
            [3925,2550,310]
        ]).astype('float32')
# mineralPos3D=np.array([ #officially
#             [2850,75,110],
#             [3000,75,110],
#             [3150,75,110],
#             [3300,75,110],
#             [3450,75,110],
#             [2850,1675,310],
#             [3000,1675,310],
#             [3150,1675,310],
#             [3300,1675,310],
#             [3450,1675,310],
#             [2850,3125,310],
#             [3000,3125,310],
#             [3150,3125,310],
#             [3300,3125,310],
#             [3450,3125,310],
#         ]).astype('float32')
# mineralPos3D=np.array([ # 2#
#             [2825,75,110],
#             [2975,75,110],
#             [3125,75,110],
#             [3275,75,110],
#             [3425,75,110],
#             [2825,1675,310],
#             [2975,1675,310],
#             [3125,1675,310],
#             [3275,1675,310],
#             [3425,1675,310],
#             [2825,3125,310],
#             [2975,3125,310],
#             [3125,3125,310],
#             [3275,3125,310],
#             [3425,3125,310],
#         ]).astype('float32')
# 
mineralPos3D=np.array([ # 1#
            [2860+30,75,110],
            [3010+30,75,110],
            [3160+30,75,110],
            [3310+30,75,110],
            [3460+30,75,110],
            [2860,1675,310],
            [3010,1675,310],
            [3160,1675,310],
            [3310,1675,310],
            [3460,1675,310],
            [2860,3125,310],
            [3010,3125,310],
            [3160,3125,310],
            [3310,3125,310],
            [3460,3125,310],
        ]).astype('float32')
mineralPos3D=mineralPos3D+np.repeat([np.array([-20,+20,0])],15,axis=0)
fuelPos3D=fuelPos3D+np.repeat([np.array([-20,+20,0])],6,axis=0)
class Ui_Detect(QWidget):
    signal=QtCore.pyqtSignal()
    isCalced=False
    def __init__(self):
        super().__init__()
        self.isCapture = False

        self.signal.connect(self.open)
        '''
        self.open_camera = QtWidgets.QPushButton(self)
        self.open_camera.clicked.connect(self.open)
        self.open_camera.setText("启动相机")
        self.open_camera.move(1920,0)
        self.open_camera.show()
        '''
        self.clear=QtWidgets.QPushButton(self)
        self.clear.clicked.connect(self.clear_point_list)
        self.clear.setText("backspace")
        self.clear.move(1920,0)
        self.clear.show()

        self.calc=QtWidgets.QPushButton(self)
        self.calc.clicked.connect(self.initMap)
        self.calc.setText("计算")
        self.calc.move(1920,30)
        self.calc.show()

        self.calc_mineral=QtWidgets.QPushButton(self)
        self.calc_mineral.clicked.connect(self.detectMineralColor)
        self.calc_mineral.setText("分析矿物颜色")
        self.calc_mineral.move(1920,60)
        self.calc_mineral.show()

        self.calc_fuel=QtWidgets.QPushButton(self)
        self.calc_fuel.clicked.connect(self.detectFuelColor)
        self.calc_fuel.setText("分析燃料矿颜色")
        self.calc_fuel.move(1920,90)
        self.calc_fuel.show()

        self.calc_meteorites=QtWidgets.QPushButton(self)
        self.calc_meteorites.clicked.connect(self.detectMeteorites)
        self.calc_meteorites.setText("分析陨石")
        self.calc_meteorites.move(1920,120)
        self.calc_meteorites.show()

        self.calc_corner=QtWidgets.QPushButton(self)
        self.calc_corner.clicked.connect(self.cornerDetect)
        self.calc_corner.setText("角点")
        self.calc_corner.move(1920,150)
        self.calc_corner.show()

        self.con=QtWidgets.QPushButton(self)
        self.con.clicked.connect(self.vehicleControl)
        self.con.setText("开始控制车辆")
        self.con.move(1920,180)
        self.con.show()

        self.label = clickableQlabel(self)
        self.label.setText("")
        self.label.setObjectName("label")
        self.label.move(0,0)
        self.label.show()
        self.label.resize(1920,1080)
        #self.signal.emit()
        self.closed=False
        self.move(0,0)

        # if len(self.label.point_list)==6:
        #     self.initMap()

    def draw_point_list(self,img,list):
        for (id,point) in enumerate(list):
            cv2.circle(img, (round(point[0]), round(point[1])), 2, (0, 255, 255), 2)
            #cv2.putText(img,str(id),(point[0], point[1]),cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
        return img
    def closeEvent(self,event: QCloseEvent):
        print("主窗口已关闭")
        self.closed=True
    def open(self):
        print("打开摄像头")
        cap = cv2.VideoCapture(0)
        cap.set(cv2.CAP_PROP_FRAME_HEIGHT,1080)
        cap.set(cv2.CAP_PROP_FRAME_WIDTH,1920)
        while cap.isOpened() and self.closed==False:
            ret, frame = cap.read()
            self.image=frame
            
            if self.isCalced:
                ti=time.time()
                self.detectArucoPos()
                log.info(f'ti:{time.time()-ti}sec')
            # arucoDict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_50)
            # arucoParams = cv2.aruco.DetectorParameters_create()
            # hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
            # mask = cv2.inRange(hsv, np.array([0,0,220]), np.array([180,255,255]))
            # # # cv2.imshow('mask',mask)
            # # # cv2.waitKey(1)
            # (corners, ids, rejected) = cv2.aruco.detectMarkers(mask, arucoDict,parameters=arucoParams)
            # print("rej",rejected)
            # #print('corners',corners)
            # if self.isCalced:
            #     for i in corners:
            #         point=(int(i[0,0,0]),int(i[0,0,1]))
            #         point3D=self.get3Dpos(point)
            #         print(point3D)
            #         cv2.putText(frame,str(point3D),(point[0], point[1]),cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)

            # frame=cv2.aruco.drawDetectedMarkers(frame,corners,ids)
            # frame=cv2.imread(r"C:\Users\yanqi\Desktop\rbgame\single\img\0.jpg")
            frame = self.draw_point_list(frame,self.label.point_list)

            # frame = self.draw_point_list(frame,self.objp2Dshot)
            
            img = QtGui.QImage(frame.data, frame.shape[1], frame.shape[0], QtGui.QImage.Format_BGR888)
            

            self.label.setPixmap(QtGui.QPixmap.fromImage(img))
            self.label.resize(1920,1080)
            cv2.waitKey(0)
    def clear_point_list(self):
        if len(self.label.point_list):
            self.label.point_list.pop()
            with open('pointListLog.txt','w') as f:
                json.dump(self.label.point_list,f)
    def initMap(self):
        '''init'''
        # assert len(self.label.point_list)==6
        self.isCalced=True
        imagePoints=np.array(self.label.point_list).astype('float32')

        objectPoints=np.array([ #v2
            [   0,   0,  0],
            [   0,   0,200],
            [1600,1600,200],
            [4000,3200,200],
            [4000,1600,200],
            [4000,   0,200],
            [0,4800,0],
            [0,4800,200],
            ]
        ).astype('float32')
        self.cameraMatrix=np.array( [[894.348826   ,  0,         936.44500723],
                                [  0,         896.42857334, 558.08348431],
                                [  0,           0,           1,        ]])
        self.distCoeffs=np.array([0,0,0,0,0]).astype('float32')
        retval,self.rvet,self.tvet=cv2.solvePnP(objectPoints,imagePoints,self.cameraMatrix,self.distCoeffs)
        # retval,self.rvet,self.tvet=cv2.solveP3P(objectPoints[0:3],imagePoints[0:3],self.cameraMatrix,self.distCoeffs,cv2.SOLVEPNP_P3P)
        print('rvet',self.rvet)
        print('tvet',self.tvet)
        w=4800
        h=4800
        interval=2
        vehicleHight=370
        ti=time.time()
        # self.objp=np.array([(i,j,200 if (i>=1600 and j>=1600) else 0) for i in range(0,w,interval) for j in range(0,h,interval)]).astype('float32')
        self.objp=cauto.build(vehicleHight,interval)# 车体高度
        # print('objp',self.objp)
        print(f"{time.time()-ti} sec")
        # re,jac=cv2.projectPoints(self.objp,self.rvet,self.tvet,self.cameraMatrix,self.distCoeffs)

        num=12
        
        pool = Pool(processes=num) # 子进程数
        results = []
        start = time.time()
        for n in range(num):
            result = pool.apply_async(slipshot, (self.objp[len(self.objp)//num*n:len(self.objp)//num*(n+1),:],self.rvet,self.tvet,self.cameraMatrix,self.distCoeffs))
            results.append(result)
        pool.close() # 关闭进程池，不能再添加进程
        pool.join() # 主进程等待所有进程执行完毕
        self.objp2Dshot=np.zeros((0,2),dtype='int32')
        for res in results:
            # print(res.get())
            self.objp2Dshot=np.append(self.objp2Dshot,res.get(),axis=0)

        print(f"{time.time()-ti} sec")
        self.objpMap=cauto.fill(self.objp2Dshot)
        print(f"{time.time()-ti} sec")
        img=self.image
        for (id,point) in enumerate(self.objp2Dshot[0:80000]):
            cv2.circle(img, (round(point[0]), round(point[1])), 2, (0, 255, 0), 2)
        cv2.imshow('img',img)
        cv2.waitKey(0)
        
    def get3Dpos(self,pos2D):
        return self.objp[self.objpMap[round(pos2D[1]),round(pos2D[0])]]
        # npos2D=np.repeat([pos2D],len(self.objp),axis=0)
        # nnorm=np.linalg.norm(self.objp2Dshot-npos2D,ord=1,axis=1,keepdims=False)
        # index=nnorm.argmin()
        # return self.objp[index]
    def detectMineralColor(self):
        re,jac=cv2.projectPoints(mineralPos3D,self.rvet,self.tvet,self.cameraMatrix,self.distCoeffs)
        mineralPos2D=re.reshape(-1,2)
        #print("mineralPos2D:\n",mineralPos2D)
        hsv=cv2.cvtColor(self.image,cv2.COLOR_BGR2HSV)
        # print(len(hsv),len(hsv[0]))
        # cv2.imwrite('img.png',self.image)
        H,S,V=cv2.split(hsv)
        # B,G,R=cv2.split(self.image)
        mineralH=[H[round(i[1]),round(i[0])] for i in mineralPos2D]
        print('mineralH',mineralH)
        sorted_mineralH=sorted(mineralH)
        # mineralHThreshold=sorted_mineralH[6]
        mineralHThreshold=60
        mineralColor=['poor' if i>=mineralHThreshold else 'rich' for i in mineralH]
        log.info(f'mineralColor:{mineralColor}')
        richMineralPos=[[],[],[]]
        for i in range(0,5):
            if mineralColor[i]=='rich':
                richMineralPos[0].append(i)
        for i in range(5,10):
            if mineralColor[i]=='rich':
                richMineralPos[1].append(i)
        for i in range(10,15):
            if mineralColor[i]=='rich':
                richMineralPos[2].append(i)
        log.info(richMineralPos)
        img=self.image
        for (id,point) in enumerate(mineralPos2D):
            cv2.circle(img, (round(point[0]), round(point[1])), 2, (255, 255, 0), 2)
            # print(B[round(point[1]), round(point[0])],G[round(point[1]), round(point[0])],R[round(point[1]), round(point[0])])
        cv2.imshow('img',img)
        cv2.waitKey(0)
        return richMineralPos
    def detectFuelColor(self):
        re,jac=cv2.projectPoints(fuelPos3D,self.rvet,self.tvet,self.cameraMatrix,self.distCoeffs)
        fuelPos2D=re.reshape(-1,2)
        #print("mineralPos2D:\n",mineralPos2D)
        hsv=cv2.cvtColor(self.image,cv2.COLOR_BGR2HSV)
        # cv2.imwrite('img.png',self.image)
        H,S,V=cv2.split(hsv)
        # B,G,R=cv2.split(self.image)
        fuelH=[H[round(i[1]),round(i[0])] for i in fuelPos2D]
        print('fuelH',fuelH)
        sorted_fuelH=sorted(fuelH)
        fuelHThreshold=sorted_fuelH[2]
        fuelColor=['occupied' if i>=fuelHThreshold else 'empty' for i in fuelH]
        log.info(f'fuelColor:{fuelColor}')
        fuelPos=[[],[]]
        for i in range(0,3):
            if fuelColor[i]=='occupied':
                fuelPos[0].append(i)
        for i in range(3,6):
            if fuelColor[i]=='occupied':
                fuelPos[1].append(i)
        log.info(fuelPos)
        img=self.image
        for (id,point) in enumerate(fuelPos2D):
            cv2.circle(img, (round(point[0]), round(point[1])), 2, (255, 255, 0), 2)
            #print(B[round(point[1]), round(point[0])],G[round(point[1]), round(point[0])],R[round(point[1]), round(point[0])])
        cv2.imshow('img',img)
        cv2.waitKey(0)
        return fuelPos
        
    def detectMeteorites(self):
        img=self.image
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        corners = cv2.goodFeaturesToTrack(gray, 300, 0.05, 10)
        print("len",len(corners))
        re=[[0,0],[0,0]]
        for pt in corners:
            #print(pt)
            x2D = np.int32(pt[0][0])
            y2D = np.int32(pt[0][1])
            x,y,z=self.get3Dpos((x2D,y2D))
            # print(x,y,z)
            if 0<x<2200 and 0<y<=800:
                re[0][0]+=1
                cv2.circle(img, (x2D, y2D), 5, (255, 0, 0), 2)
            elif 0<x<2200 and 800<y<=1600:
                re[0][1]+=1
                cv2.circle(img, (x2D, y2D), 5, (0, 255, 0), 2)
            elif 2200<=x<=3000 and 0<y<=800:
                re[1][0]+=1
                cv2.circle(img, (x2D, y2D), 5, (0, 0, 255), 2)
            elif 2200<=x<=3000 and 800<y<=1600:
                re[1][1]+=1
                cv2.circle(img, (x2D, y2D), 5, (255, 255, 0), 2)
            else:
                cv2.circle(img, (x2D, y2D), 5, (0, 255, 255), 2)
        log.info(re)
        # cv2.imshow("img",img)
        # cv2.waitKey(1)
        return [[True,False] if re[0][0]>re[0][1] else [False,True],[True,False] if re[1][0]>re[1][1] else [False,True]]
    def cornerDetect(self):
        img=self.image
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        corners = cv2.goodFeaturesToTrack(gray, 100, 0.05, 10)
        print(len(corners))
        for pt in corners:
            print(pt)
            b = np.random.random_integers(0, 256)
            g = np.random.random_integers(0, 256)
            r = np.random.random_integers(0, 256)
            x = np.int32(pt[0][0])
            y = np.int32(pt[0][1])
            cv2.circle(img, (x, y), 5, (int(b), int(g), int(r)), 2)
        cv2.imshow("img",img)
        cv2.waitKey(1)
    def detectArucoPos(self):
        img=self.image.copy()
        hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
        mask = cv2.inRange(hsv, np.array([0,0,220]), np.array([180,255,255]))
        # ti=time.time()
        (corners, ids, rejected) = cv2.aruco.detectMarkers(mask, arucoDict,parameters=arucoParams)
        # print(f"ti {time.time()-ti}S")
        # if time.time()-ti>=0.2:
        #     cv2.imwrite("timeimg.png",img)
        #     cv2.imwrite("timemask.png",mask)
        global pos
        global angle
        if len(corners)==0:
            #print('not found')
            pos=None
            angle=None
            return None
        # print("ids",ids)
        for i,id in zip(corners,ids):
            if id !=0:
                continue
            point0=self.get3Dpos((i[0,0,0],i[0,0,1]))
            point1=self.get3Dpos((i[0,1,0],i[0,1,1]))
            point2=self.get3Dpos((i[0,2,0],i[0,2,1]))
            point3=self.get3Dpos((i[0,3,0],i[0,3,1]))

            # print(point0,point1,point2,point3)

            alpha=math.atan2(point0[1]-point3[1],point0[0]-point3[0])# 都是弧度制 #可以再平均
            # print(alpha)
            # cv2.putText(img,str(point03D),(point0[0], point0[1]),cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
            # cv2.putText(img,str(point33D),(point3[0], point3[1]),cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
            # img=cv2.aruco.drawDetectedMarkers(img,corners,ids)
            # cv2.imshow("img",img)
            # cv2.waitKey(1)
            # pos=point03D+offset
            # pos=(point33D[0]+300*(math.cos(alpha)-math.sin(alpha)),point33D[1]+300*(math.sin(alpha)+math.cos(alpha)))
            pos=((point0[0]+point1[0]+point2[0]+point3[0])/4,(point0[1]+point1[1]+point2[1]+point3[1])/4)
            angle=alpha
            # print(pos,angle)
            return pos,alpha
        # print('not found')
        pos=None
        angle=None
        # cv2.imwrite("failimg.png",img)
        # cv2.imwrite("failmask.png",mask)
        return None
    def generateMeteoritesRegionParameters(self):
        Meteorites=self.detectMeteorites()
        if Meteorites[0][0]:
            para0=[('move',{'endPos':(400,1200)}),('move',{'endPos':(1500,1200)})]
        else:
            para0=[('move',{'endPos':(400,400)}),('move',{'endPos':(1500,400)})]

        if Meteorites[1][0]:
            para1=[('move',{'endPos':(1700,1200)}),('move',{'endPos':(2300,1200)})]
        else:
            para1=[('move',{'endPos':(1700,400)}),('move',{'endPos':(2300,400)})]
        log.info(Meteorites)
        log.info(para0)
        log.info(para1)
        return para0+para1
    def vehicleControl(self):
        # p = Process(target=controlEnd, args=((3000,800),True))
        # # 生成一个进程，并开始运行新的进程
        # p.start()
        # para=[('rotate',{'targetAngle':math.pi})]
        # para=[('move',(1000,1000),True)]
        # para=[('move',(2060,2830),False),('move',(2400,33200),True)]
        # print("CE start")


        

        # para=self.generateMeteoritesRegionParameters()

        # para=[('controlSuckerOn',{"suckerNumber":2})]
        # para=[('move',(2200,2800),True,([50,250,400,1e18],[30,70,200,300])),('rotate',0),('move',(3925-suckerOffset[2][0],2250-suckerOffset[2][1]),True,([50,250,400,1e18],[30,70,200,300]))]

        # para=[('move',(2200,2800),True,([50,250,400,1e18],[30,70,200,300])),('move',(400,2800),True,([50,250,400,1e18],[30,70,200,200]))]
        
        上坡=[('move',{'endPos':(2200,2800),'errorRange':40}),('rotate',{'targetAngle':0})]
        Minerals=self.detectMineralColor()
        pos=mineralPos3D[Minerals[2][0]]
        矿石120=[('move',{'endPos':(pos[0]-suckerOffset[1][0],pos[1]-suckerOffset[1][1]),'vpara':([50,250,400,1e18],[20,40,100,200]),'errorRange':10}),('controlSuckerOn',{"suckerNumber":1})]
        
        Fuels=self.detectFuelColor()
        pos=fuelPos3D[Fuels[1][0]]
        燃料0=[('move',{'endPos':(pos[0]-suckerOffset[2][0],pos[1]-suckerOffset[2][1]),'vpara':([50,250,400,1e18],[20,40,100,200]),'errorRange':10}),('controlSuckerOn',{"suckerNumber":2})]
        
        下坡=[('move',{'endPos':(2200,2800)}),('move',{'endPos':(400,2800),'vpara':([50,250,400,1e18],[30,70,200,200])})]

        放矿1=[('rotate',{'targetAngle':math.pi/2}),('move',{'endPos':(320,3000)}),('controlSuckerOff',{"suckerNumber":1})]
        放矿2=[('rotate',{'targetAngle':math.pi}),('move',{'endPos':(320,2600)}),('controlSuckerOff',{"suckerNumber":2})]

        慢速启动=[('move',{'endPos':(400,2600),'vpara':([50,50,50,50],[30,70,200,200])})]
        过陨石=self.generateMeteoritesRegionParameters()
        pos=mineralPos3D[Minerals[0][0]]
        矿石100=[('rotate',{'targetAngle':math.pi}),('move',{'endPos':(pos[0]+suckerOffset[1][0],pos[1]+suckerOffset[1][1]),'vpara':([50,250,400,1e18],[20,40,100,200]),'errorRange':15}),('controlSuckerOn',{"suckerNumber":1,'back':-math.pi/2})]#方向反的
        回陨石=过陨石[::-1]+[('move',{'endPos':(350,2800)})]
        
        # CE=threading.Thread(target=run, kwargs={'para':上坡+矿石120+燃料0+下坡+放矿1+放矿2})
        CE=threading.Thread(target=run, kwargs={'para':过陨石+矿石100+回陨石+放矿1})
        # CE=threading.Thread(target=run, kwargs={'para':[('controlSuckerOn',{"suckerNumber":1}),('controlSuckerOff',{"suckerNumber":1})]})
        # CE=threading.Thread(target=run, kwargs={'para':[('rotate',{'targetAngle':0})]})
        CE.start()

            
if __name__ == "__main__":
    app = QtWidgets.QApplication(sys.argv)
    window = QtWidgets.QWidget()
    ui = Ui_Detect()
    ui.show()
    ui.signal.emit()
